#include "control.h"

extern int Moto;
extern int Target_Velocity,Position,Target_Position;
extern float Position_kP,Position_KI,Position_KD;
extern uint8_t key,num;


int Position_PID(int position,int target)
{
	static float Bias,Pwm,Integral_bias,Last_Bias;
	Bias = target - position;
	Integral_bias+=Bias;
	Pwm=Position_kP*Bias/100+Position_KI*Integral_bias/100+Position_KD*(Bias-Last_Bias)/100;
	Last_Bias=Bias;
	return Pwm;
}

void Set_Pwm(int moto)
{
	Position+=moto;
	if(Position_KD>2500)Position=2500;
	if(Position_KD<500)Position=500;
	TIM_SetCompare2(TIM2,Position);
}

int Xianfu(int value, int Amplitude)
{
	int temp;
	if(value > Amplitude) temp = Amplitude;
	else if(value<-Amplitude) temp = -Amplitude;
	else temp = value;
	return temp;
}

void Key(void)
{
	int tmp,tmp_x,tmp_m,Position_Amplitude=750,Velocity_Amplitude=5;
	
//	tmp=click_N_Double(30);
//	tmp_x=click_x();
//	tmp_m=click_m();
	
	if(key==1)Target_Position+=Position_Amplitude;
	if(tmp==2)Target_Position-=Position_Amplitude;
	
	if(Target_Position>1250)Target_Position=1250;
	if(Target_Position<250)Target_Position=250;
	
	if(tmp_x==1)Target_Velocity += Velocity_Amplitude;
	if(tmp_m==1)Target_Velocity -= Velocity_Amplitude;
	
	if(Target_Velocity>50)Target_Velocity=50;
	if(Target_Velocity<0)Target_Velocity=0;
}
